#include "Physics2d.h"

#include "stdio.h"

bool pushOut( TBall &a, const TBall &b ) {
    double d = a.radius + b.radius;
    if ( fabs(a.pos.getX() - b.pos.getX()) > d ) return false;
    if ( fabs(a.pos.getY() - b.pos.getY()) > d ) return false;
    if ( (a.pos - b.pos).lenght() < d ) {
        double off = d - (a.pos - b.pos).lenght();
        CVect2d fix = (a.pos - b.pos).direction();
        fix *= off + 0.0001;
        a.pos += fix;

        CVect2d bounce = a.speed.projectionTo(fix)*-1;
        CVect2d slide = a.speed.projectionTo(fix.rightNormal());
        double friction = 0.02;
        double bounciness = 0.8;
        a.speed = (bounce * bounciness) + (slide*(1.0-friction));

        return true;
    } else {
        return false;
    }
}

bool pushOut( TBall &a, const TBox &b ){

    // tikrinti visas ašis.
    // jei toje ešyje kūnai nesikerta grąžian false
    // jei visos ašys kertasi grąžin true;

    CVect2d rtrn[3];
    double d;

    // X ašis
    d = a.radius+b.hsize.getX() - fabs(a.pos.getX() - b.pos.getX());
    if ( d < 0 )
        return false;
    else {
        rtrn[0].set( a.pos.getX() - b.pos.getX()+0.001, 0.0 );
        rtrn[0].normalize();
        rtrn[0] *= (d+0.0001);
    }

    // Y ašis
    d = a.radius+b.hsize.getY() - fabs(a.pos.getY() - b.pos.getY());
    if ( d < 0 )
        return false;
    else {
        rtrn[1].set( 0.0, a.pos.getY() - b.pos.getY()+0.001 );
        rtrn[1].normalize();
        rtrn[1] *= (d+0.0001);
    }

    // šonai
    if ( fabs(a.pos.getX() - b.pos.getX()) > b.hsize.getX()
        && fabs(a.pos.getY() - b.pos.getY()) > b.hsize.getY() ) {

        CVect2d corner;
        if ( a.pos.getX() > b.pos.getX() )
            corner.setX( b.pos.getX() + b.hsize.getX() );
        else
            corner.setX( b.pos.getX() - b.hsize.getX() );
        if ( a.pos.getY() > b.pos.getY() )
            corner.setY( b.pos.getY() + b.hsize.getY() );
        else
            corner.setY( b.pos.getY() - b.hsize.getY() );

        d = a.radius - (a.pos-corner).lenght();
        if ( d < 0 )
            return false;
        else {
            rtrn[2] = (a.pos - corner).direction() * (d+0.0001);
        }
    } else {
        rtrn[2].set( 1000.0, 1000.0 );
    }

    // dalys susiduria, reikia rasti trumpiausią gražinimo vektorių;
    double x = rtrn[0].lenght(),
           y = rtrn[1].lenght(),
           z = rtrn[2].lenght();
    CVect2d fix;
    if ( x < y && x < z ) {
        fix = rtrn[0];
    } else if ( y < z ) {
        fix = rtrn[1];
    } else
        fix = rtrn[2];

    a.pos += fix;
    CVect2d bounce = a.speed.projectionTo(fix)*-1;
    CVect2d slide = a.speed.projectionTo(fix.rightNormal());
    double friction = 0.02;
    double bounciness = 0.8;
    a.speed = (bounce * bounciness) + (slide*(1.0-friction));


    return true;
}

// Tikrina ar apskritimas susiduria su kitu apskritimu
bool elasticCollision( TBall &a, TBall &b ) {
    double d = a.radius + b.radius;
    if ( fabs(a.pos.getX() - b.pos.getX()) > d ) return false;
    if ( fabs(a.pos.getY() - b.pos.getY()) > d ) return false;
    if ( (a.pos - b.pos).lenght() > d ) return false;

    double off = d - (a.pos - b.pos).lenght();
    CVect2d fix = (a.pos - b.pos).direction();
    fix *= off + 0.0001;
    a.pos += fix;

    CVect2d dir = (b.pos-a.pos).direction();
    CVect2d as = a.speed, bs = b.speed;
    a.speed = as.projectionTo(dir.leftNormal()) + bs.projectionTo(dir);
    b.speed = bs.projectionTo(dir.rightNormal()) + as.projectionTo(dir);

    // Kamuoliukai susiduria

    return true;
}

bool collide( const CVect2d &aPos, const CVect2d &bPos, const double &aRad, const double &bRad )
{
    double d = aRad + bRad;
    if ( fabs(aPos.getX() - bPos.getX()) > d ) return false;
    if ( fabs(aPos.getY() - bPos.getY()) > d ) return false;
    if ( (aPos - bPos).lenght() > d ) return false;

    return true;
}

// Tirina ar apskritimas susiduria su stačiakampiu
bool collide( const TBall &a, const TBox &b ){

    // tikrinti visas ašis.
    // jei toje ešyje kūnai nesikerta grąžian false
    // jei visos ašys kertasi grąžin true;

    // X ašis
    if ( fabs(a.pos.getX() - b.pos.getX()) > a.radius+b.hsize.getX() )
        return false;

    // Y ašis
    if ( fabs(a.pos.getY() - b.pos.getY()) > a.radius+b.hsize.getY() )
        return false;

    // šonai
    if ( fabs(a.pos.getX() - b.pos.getX()) > b.hsize.getX()
        && fabs(a.pos.getY() - b.pos.getY()) > b.hsize.getY() ) {

        CVect2d corner;
        if ( a.pos.getX() > b.pos.getX() )
            corner.setX( b.pos.getX() + b.hsize.getX() );
        else
            corner.setX( b.pos.getX() - b.hsize.getX() );
        if ( a.pos.getY() > b.pos.getY() )
            corner.setY( b.pos.getY() + b.hsize.getY() );
        else
            corner.setY( b.pos.getY() - b.hsize.getY() );

        if ( (a.pos-corner).lenght() > a.radius )
            return false;
    }

    return true;
}

double pointDisToLine( const CVect2d &p, const CVect2d &lp1, const CVect2d &lp2 )
{
    CVect2d norm = (lp1 - lp2).rightNormal().direction();
    return norm * ( lp1-p ).projectionTo(norm);
}

bool collide( TBall &a, const TTriangle &t )
{
    CVect2d rtrn[4];    // grąžinimo vektoriai
    double d[3];    // atstumas iki trikampio sienų
    d[0] = pointDisToLine( a.pos, t.p[0], t.p[1] );
    if ( d[0] > a.radius ) return false;
    d[1] = pointDisToLine( a.pos, t.p[1], t.p[2] );
    if ( d[1] > a.radius ) return false;
    d[2] = pointDisToLine( a.pos, t.p[2], t.p[0] );
    if ( d[2] > a.radius ) return false;

    CVect2d n[3];    // trikampio normalės
    n[0] = (t.p[1]-t.p[0]).rightNormal();
    n[1] = (t.p[2]-t.p[1]).rightNormal();
    n[2] = (t.p[0]-t.p[2]).rightNormal();

    if ( d[0] < 0 && d[1] < 0 && d[2] < 0 ) { // apskritimas yra trikampio viduje
        if ( d[0] > d[1] && d[0] > d[2] ) {
            rtrn[0] = n[0].direction() * (a.radius-d[0]+0.0001);
            d[1] = 1000.0; d[2] = 1000.0;
        } else if ( d[1] > d[2] ) {
            rtrn[1] += n[1].direction() * (a.radius-d[1]+0.0001);
            d[0] = 1000.0; d[2] = 1000.0;
        } else {
            rtrn[2] += n[2].direction() * (a.radius-d[2]+0.0001);
            d[0] = 1000.0; d[1] = 1000.0;
        }
    } else {
        if ( d[0] < a.radius && d[0] > 0 )
            rtrn[0] = n[0].direction() * (a.radius-d[0]+0.0001);
        else
            d[0] = 1000.0;

        if ( d[1] < a.radius && d[1] > 0 )
            rtrn[1] = n[1].direction() * (a.radius-d[1]+0.0001);
        else
            d[1] = 1000.0;

        if ( d[2] < a.radius && d[2] > 0 )
            rtrn[2] = n[2].direction() * (a.radius-d[2]+0.0001);
        else
            d[2] = 1000.0;
    }

    double s[6];    // atstumas iki trikampio sienų statinių
    s[0] = pointDisToLine( a.pos, t.p[0], t.p[0]+n[0]);
    s[1] = pointDisToLine( a.pos, t.p[0], t.p[0]+n[2]);
    s[2] = pointDisToLine( a.pos, t.p[1], t.p[1]+n[1]);
    s[3] = pointDisToLine( a.pos, t.p[1], t.p[1]+n[0]);
    s[4] = pointDisToLine( a.pos, t.p[2], t.p[2]+n[2]);
    s[5] = pointDisToLine( a.pos, t.p[2], t.p[2]+n[1]);

    double len = 1000.0;    // atstumas iki trikampio kampo
    rtrn[3].set( 1000.0, 1000.0 );

    if ( s[0] > 0 && s[1] < 0 ) {   // t.p[0] yra arčiausias trikampio taškas;
        len = (a.pos - t.p[0]).lenght();
        if ( len > a.radius )
            return false;
        else
            rtrn[3] = (a.pos-t.p[0]).direction()*(a.radius-len);
    }
    if ( s[2] > 0 && s[3] < 0 ) {   // t.p[1] yra arčiausias trikampio taškas;
        len = (a.pos - t.p[1]).lenght();
        if ( len > a.radius )
            return false;
        else
            rtrn[3] = (a.pos-t.p[1]).direction()*(a.radius-len);
    }
    if ( s[4] > 0 && s[5] < 0 ) {   // t.p[2] yra arčiausias trikampio taškas;
        len = ( a.pos - t.p[2]).lenght();
        if ( len > a.radius )
            return false;
        else
            rtrn[3] = (a.pos-t.p[2]).direction()*(a.radius-len);
    }

    len -= a.radius;

    CVect2d fix;

    if ( len < d[0] && len < d[1] && len < d[2] ) {
        fix = rtrn[3];
    } else if ( d[0] < d[1] && d[0] < d[2] ) {
        fix = rtrn[0];
    } else if ( d[1] < d[2] ) {
        fix = rtrn[1];
    } else
        fix = rtrn[2];

    a.pos += fix;
    CVect2d bounce = a.speed.projectionTo(fix)*-1;
    CVect2d slide = a.speed.projectionTo(fix.rightNormal());
    double friction = 0.02;
    double bounciness = 0.8;
    a.speed = (bounce * bounciness) + (slide*(1.0-friction));

    
    // apskritimas susiduria su trikampiu, reikia jį išstumti
    

//    if ( d[0] > a.radius || d[1] > a.radius || d[2] > a.radius )
//        return false;

    return true;
}

bool inHole( const TBall &a, const TBall &b )
{
    return ( (a.pos-b.pos) < b.radius-a.radius );
}

double circleDisToSpline( CVect2d cPos, double cRad, CVect2d cDir, CVect2d p1, CVect2d p2 )
{
    if ( ((p1-p2).rightNormal() * cDir) <= 0 )
        return -1.0;
    double AD = pointDisToLine( cPos, p1, p2 );
    if ( AD < 0 )
        return -1.0;
    double AC = AD - cRad;
    double AB = AC / (cDir*((p1-p2).rightNormal().direction()));
    CVect2d F = cPos + cDir*AB +(p1-p2).rightNormal().direction()*cRad;
    double p1F = pointDisToLine( F, p1, p1 + (p1-p2).rightNormal() );
    CVect2d p;
    if ( p1F > 0 && p1F < (p1-p2).lenght() ) {
        return AB;
    } else if ( p1F < 0 ) {
        p = p1;
    } else {
        p = p2;
    }

    double pL = fabs(pointDisToLine( p, cPos, cPos+cDir ));
    if ( pL > cRad )
        return -1.0;
    double Jp = (cPos - p).lenght();
    double JL = sqrt( Jp*Jp - pL*pL );
    double Mp = cRad;
    double ML = sqrt( Mp*Mp - pL*pL );
    double JM = JL - ML;
    return JM;
}

CVect2d circleCPOnSpline( CVect2d cPos, double cRad, CVect2d cDir, CVect2d p1, CVect2d p2 )
{
    if ( ((p1-p2).rightNormal() * cDir) <= 0 )
        return CVect2d(0.0,0.0);
    double AD = pointDisToLine( cPos, p1, p2 );
    if ( AD < 0 )
        return CVect2d(0.0,0.0);
    double AC = AD - cRad;
    double AB = AC / (cDir*((p1-p2).rightNormal().direction()));
    CVect2d F = cPos + cDir*AB +(p1-p2).rightNormal().direction()*cRad;
    double p1F = pointDisToLine( F, p1, p1 + (p1-p2).rightNormal() );
    CVect2d p;
    if ( p1F > 0 && p1F < (p1-p2).lenght() ) {
        return F;
    } else if ( p1F < 0 ) {
        return p1;
    } else {
        return p2;
    }
}

double circleToMovingCircle( CVect2d aPos, CVect2d bPos, CVect2d aSpeed, CVect2d bSpeed, double aRad, double bRad )
{
    double xd = aPos.getX() - bPos.getX();
    double yd = aPos.getY() - bPos.getY();
    double xsd = aSpeed.getX() - bSpeed.getX();
    double ysd = aSpeed.getY() - bSpeed.getY();

    double A = xsd*xsd + ysd*ysd;
    double B = 2.0 * ( xsd*xd + ysd*yd );
    double C = xd*xd + yd*yd - pow(aRad+bRad,2);
    double D = B*B - 4*A*C;

    double t;

    if ( D < 0 )
        return -1.0;
    else if ( D == 0 )
        t = -1.0*B / (2.0*A);
    else
        t = (-1.0*B - sqrt(D)) / (2.0*A);

    if ( t > 0 )
        return t;
    else
        return -1.0;
}
